/*
 * @Author: wangyingjie 2778809626@qq.com
 * @Date: 2025-07-14 15:36:05
 * @LastEditors: wangyingjie 2778809626@qq.com
 * @LastEditTime: 2025-07-20 08:48:04
 * @FilePath: /two_wheeled_car/src/bot_control/src/sensor_reader.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>

class SensorReader : public rclcpp::Node
{
public:
    SensorReader() : Node("sensor_reader")
    {
        // 创建激光雷达订阅
        lidar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/lidar/scan", 10,
            [this](const sensor_msgs::msg::LaserScan::SharedPtr msg)
            {
                RCLCPP_INFO(get_logger(), "Lidar data: min_range=%.2f, max_range=%.2f, front=%.2f",
                            msg->range_min, msg->range_max, msg->ranges[msg->ranges.size() / 2]);
            });

        // 创建IMU订阅
        imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
            "/imu", 10,
            [this](const sensor_msgs::msg::Imu::SharedPtr msg)
            {
                auto &o = msg->orientation;
                RCLCPP_INFO(get_logger(), "IMU orientation: x=%.2f, y=%.2f, z=%.2f, w=%.2f",
                            o.x, o.y, o.z, o.w);
            });

        // 创建里程计订阅
        odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            [this](const nav_msgs::msg::Odometry::SharedPtr msg)
            {
                auto &p = msg->pose.pose.position;
                RCLCPP_INFO(get_logger(), "Position: x=%.2f, y=%.2f, z=%.2f",
                            p.x, p.y, p.z);
            });
                // 创建激光雷达订阅
        radar_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
            "/radar/scan", 10,
            [this](const sensor_msgs::msg::LaserScan::SharedPtr msg)
            {
                RCLCPP_INFO(get_logger(), "radar data: min_range=%.2f, max_range=%.2f, front=%.2f",
                            msg->range_min, msg->range_max, msg->ranges[msg->ranges.size() / 2]);
            });
    }

private:
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr radar_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SensorReader>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}